import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import PoseStamped
from cv_bridge import CvBridge
import socket
import struct
import pickle
import threading
import time
import numpy as np
import cv2

class ImageSenderNode(Node):
    def __init__(self):
        super().__init__('image_sender_node')
        self.get_logger().info("Image Sender Node initialized.")

        # 创建发布器
        self.obj_pose_pub = self.create_publisher(PoseStamped, '/obj_pose', 10)
        self.button_pose_pub = self.create_publisher(PoseStamped, '/button_pose', 10)
        self.machine_cup_pose_pub = self.create_publisher(PoseStamped, '/machine_cup_pose', 10)
        self.handover_pose_pub = self.create_publisher(PoseStamped, '/handover_pose', 10)

        # 创建订阅器
        self.color_img_sub = self.create_subscription(Image, "/camera/color/image_raw", self.color_callback, 1)
        self.depth_img_sub = self.create_subscription(Image, "/camera/depth/image_raw", self.depth_callback, 1)

        # 初始化全局变量
        self.color_img_msg = None
        self.depth_img_msg = None
        self.bridge = CvBridge()

        # 创建TCP套接字
        self.server_ip = '192.168.2.234'
        self.server_port = 12347
        self.client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.client_socket.connect((self.server_ip, self.server_port))
        self.get_logger().info(f"Connected to server {self.server_ip}:{self.server_port}")

        # 启动线程
        self.receive_thread = threading.Thread(target=self.receive_pose_from_server)
        self.receive_thread.daemon = True
        self.receive_thread.start()

        self.image_thread = threading.Thread(target=self.image_send_thread)
        self.image_thread.daemon = True
        self.image_thread.start()

    def color_callback(self, msg):
        self.color_img_msg = msg

    def depth_callback(self, msg):
        self.depth_img_msg = msg

    def image_send_thread(self):
        try:
            while rclpy.ok():
                if self.color_img_msg is None or self.depth_img_msg is None:
                    continue
                color_image = self.bridge.imgmsg_to_cv2(self.color_img_msg, desired_encoding="bgr8")
                depth_image = self.bridge.imgmsg_to_cv2(self.depth_img_msg, desired_encoding="16UC1")
                timestamp = time.time()
                # 发送数据逻辑...
        except Exception as e:
            self.get_logger().error(f"Error in image send thread: {e}")

    def receive_pose_from_server(self):
        try:
            while rclpy.ok():
                # 接收数据逻辑...
                pass
        except Exception as e:
            self.get_logger().error(f"Error in receive pose thread: {e}")

def main(args=None):
    rclpy.init(args=args)
    node = ImageSenderNode()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
